#include "./motor_drv.h"
#include "../device/gpio.h"
#include "../rtos/rtos.h"

namespace driver {

using namespace device;

using SLEEP = PC<4>; // low
using RESET = PC<5>; // high

using MISO = PA<2>;
using MOSI = PA<1>;
using CS1 = PE<8>;
using CS2 = PB<8>;
using SCLK = PB<7>;

static void delay() {
    for (volatile uint32_t i = 0; i < 50; i ++);
}

template <typename CS>
static uint16_t translate(uint16_t value) {
    uint16_t ret = 0;
    CS::Set();
    delay();
    for (uint8_t i = 0; i < 16; i ++) {
        if (value & 0x8000) {
            MOSI::Set();
        } else {
            MOSI::Clr();
        }
        delay();
        SCLK::Set();
        delay();
        value <<= 1;
        ret <<= 1;
        if (MISO::Read()) {
            ret ++;
        }
        SCLK::Clr();
        delay();
    }
    CS::Clr();
    return ret & 0xFFF;
}

MotorDrv::MotorDrv() {
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN |
        RCC_AHB1ENR_GPIOAEN |
        RCC_AHB1ENR_GPIOEEN |
        RCC_AHB1ENR_GPIOBEN;

    ConfigOutput<SLEEP, RESET, MOSI, CS1, CS2, SCLK>();
    ConfigFloatInput<MISO>();
    reset();
    wake_up();
}

void MotorDrv::sleep() {
    SLEEP::Clr();
}

void MotorDrv::wake_up() {
    SLEEP::Set();
    osDelay(10);
}

void MotorDrv::reset() {
    RESET::Set();
    osDelay(10);
    RESET::Clr();
}

void MotorDrv::write1(uint8_t addr, uint16_t value) {
    rtos::Mutex::Auto __(m_mutex);
    translate<CS1>((addr << 12) + value);
}

void MotorDrv::write2(uint8_t addr, uint16_t value) {
    rtos::Mutex::Auto __(m_mutex);
    translate<CS2>((addr << 12) + value);
}

uint16_t MotorDrv::read1(uint8_t addr) {
    rtos::Mutex::Auto __(m_mutex);
    return translate<CS1>((addr | 0x08) << 12);
}

uint16_t MotorDrv::read2(uint8_t addr) {
    rtos::Mutex::Auto __(m_mutex);
    return translate<CS2>((addr | 0x08) << 12);
}

}
